
package org.usfirst.frc.team4188.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
	
	RobotDrive robotDrive;
	RobotDrive lift;
	Joystick driveStick;
	Joystick liftStick;
	Relay spike;
	AxisType twistAxis;
	

		
	
	
	
    public void robotInit() {
    	robotDrive = new RobotDrive(0,1,2,3); //PWM ports 0,1,2,3
    	lift = new RobotDrive(4, 5);
    	driveStick = new Joystick(0);
    	liftStick = new Joystick(1);
    	spike = new Relay(0);
    	
    	driveStick.setAxisChannel(twistAxis, 3);
    	//driveStick.getRawAxis(3);

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
    	
    	robotDrive.mecanumDrive_Cartesian(driveStick.getX(), driveStick.getY(), driveStick.getZ(), 0);
    	 
    	 lift.arcadeDrive(liftStick);
         if(liftStick.getRawButton(1)){
         	
         	spike.set(Relay.Value.kOn);
         }
         else{
         	
 			spike.set(Relay.Value.kOff);
         }
    	
    	
    	
    	
        
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    	LiveWindow.run();
    
    }
    
}
